#include "ServiceOperate.h"
#include <get_srvs_from_json.h>
#include <get_msgs_from_json.h>
#include <agv_msgs_converter.h>
#include <regex>
#include <shared_parameters/shared_params_interface.hpp>

#define SERVICE_DYN_PARAM "/DynParam"                                          // 动态参数发送
#define SERVICE_STATIC_PARAM "/StaticParam"                                    // 静态参数写入与读取
#define SERVICE_CHANGE_NAV_TYPE "/change_nav_type"                             // 切换导航方式
#define SERVICE_LOCAL_ORDER "/local_order"                                     // 单机任务下发
#define SERVICE_VDA5050_INSTANTACTIONS "/vda5050_instantActions"               // 单机任务操作
#define SERVICE_LIDAR_CALIBRATION_SERVICE "/lidarCalibration_service"          // 单激光雷达标定
#define SERVICE_QR_CALIBRATION_SERVICE "/qrCalibration_service"                // 二维码相机标定
#define SERVICE_SET_LASER_CALIB "/setLaserCalib"                               // 更新雷达激光雷达参数
#define SERVICE_AGV_CALIB_ODOM_CALIB "/agv_calib/odom_calib"                   // 里程计标定动作指令
#define SERVICE_GET_REFLECTOR_MAP_LISTS "/get_reflector_map_lists"             // 反光柱地图名称列表(txt)
#define SERVICE_START_REFLECTOR_NAV "/start_reflector_nav"                     // 反光柱导航模式切换：定位/建图/更新
#define SERVICE_SAVE_REFLECTOR_MAP "/save_reflector_map"                       // 反光柱地图建图/编辑时保存
#define SERVICE_SET_REFLECTOR_INITIALPOSE "/set_reflector_initialpose"         // 反光柱导航定位设置起始坐标
#define SERVICE_OPERATION_MODE "/operation_mode"                               // 控制方式切换
#define SERVICE_PARAM_CONFIG "/param_config"                                   // 参数/避障文件校验回复
#define SERVICE_OUTPUTINTERFACE "/outputinterface"                             // do口控制
#define SERVICE_LOG_CONFIG "/log_config"                                       // 黑盒文件列表查询
#define SERVICE_COMPRESS_DATA "/compress_data"                                 // 黑盒触发
#define SERVICE_EDIT_MAP "/edit_map"                                           // 编辑激光地图
#define SERVICE_LOCATION_MODE "/location_mode"                                 // 激光地图模式切换，启动建图/定位
#define SERVICE_SAVE_MAP "/save_map"                                           // 建图保存SLAM地图
#define SERVICE_AGV_RELOCATE "/agv_relocate"                                   // 激光地图激光上线重定位
#define SERVICE_START_AUTOCALIB "/start_autocalib"                             // 双激光自动标定
#define SERVICE_SET_FILTER_PARAM "/set_filter_param"                           // 标定点云滤波
#define SERVICE_GET_MAP_PNG "/get_map_png"                                     // 请求SLAM地图png
#define SERVICE_GET_MAP_LIST "/get_map_list"                                   // 请求SLAM地图名称列表
#define SERVICE_CHANGE_MAP "/change_map"                                       // 激光地图删除/重命名
#define SERVICE_GET_LANDMARKLISTS "/get_LandmarkLists"                         // 获取激光地图反光柱信息
#define SERVICE_FW_VERSION "/fw_version"                                       // 获取固件版本
#define SERVICE_FW_UPDATE "/fw_update"                                         // 请求开始固件更新
#define SERVICE_FTP_REQUEST "/ftp_request"                                     // 固件更新请求打开ftp
#define SERVICE_SYNC_SLAM_MAP "/sync_slam_map"                                 // 同步slam地图校验
#define SERVICE_SYNC_REFLECTOR_SLAM_MAP "/sync_reflector_slam_map"             // 同步反光地图校验
#define SERVICE_AGV_AVOID_UPDATE "/agv_avoid_update"                           // 触发避障文件更新生效
#define SERVICE_AGV_OBSTACLE_AVOIDANCE_SERVER "/agv_obstacle_avoidance_server" // 切换避障方案

#define SERVICE_GET_PARAM "/rosapi/get_param" // 读取ROS参数服务器值(无需call服务)
#define SERVICE_SET_PARAM "/rosapi/set_param" // 设置ROS参数服务器值(无需call服务)
// #define SERVICE_CHANGE_REFLECTOR_MAP "/change_reflector_map" // 反光柱导航删除/重命名地图(暂未实现)

#define PUB_TOPIC_HALFAUTO_MOVE "/halfauto_allow_move"                          // 遥控话题：半自动状态允许移动 std_msgs::msg::Bool
#define PUB_TOPIC_KEYBOARD_VEL  "/twist_mux/keyboard_vel"                       // 遥控话题：普通遥控 geometry_msgs::msg::Twist
#define PUB_TOPIC_KEYBOARD_OVERRIDE "/twist_mux/keyboard_override_vel"          // 遥控话题：高权限遥控（新）geometry_msgs::msg::Twist
#define PUB_TOPIC_KEYBOARD_TELEOP "/twist_mux/command_teleop_override_joy"      // 遥控话题：高权限遥控（旧）geometry_msgs::msg::Twist
#define PUB_TOPIC_PERIPH_TELEOP_INFO "/base/periphTeleopInfo"                   // 遥控话题：外设动作控制

CServiceOperate::CServiceOperate(std::shared_ptr<rclcpp::Node> nh) : m_nh(nh)
{
    init();
}

CServiceOperate::~CServiceOperate() {};

void CServiceOperate::setSendDataProcessor(std::function<void(std::vector<int>, std::string)> processor)
{
    m_vSendDataProcessor = processor;
}

void CServiceOperate::recvJsonData_call(const int id, const Json::Value &root)
{
    RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"), "client " << id << ",srv call: " << root);
    std::string srvName = root["service"].asString();
    Json::Value json_args;
    if (root.isMember("args"))
    {
        json_args = root["args"];
    }
    bool result = false;
    Json::Value json_values = callService(srvName, json_args, result);

    Json::Value json_resp;
    json_resp["op"] = "service_response";
    json_resp["service"] = srvName;
    json_resp["values"] = json_values;
    json_resp["result"] = result;

    std::string str_resp = Json::FastWriter().write(json_resp);
    if (!str_resp.empty() && str_resp.back() == '\n')
    {
        str_resp.pop_back();
    }
    m_vSendDataProcessor({id}, str_resp);

    RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"),"client " << id << ",srv response: " << str_resp);
}

void CServiceOperate::recvJsonData_pub(const Json::Value &root)
{
    if (!root.isMember("msg"))
    {
        return;
    }
    Json::Value json_msg = root["msg"];
    std::string topicName = root["topic"].asString();
    if (topicName == PUB_TOPIC_HALFAUTO_MOVE)
    {
        publishMsg(0, get_std_msgs_Bool(json_msg));
    }
    else if (topicName == "/base/twist_mux/keyboard_vel")
    {
        publishMsg(1, get_geometry_msgs_TwistStamped(json_msg));
    }
    else if (topicName == "/base/twist_mux/keyboard_override_vel")
    {
        publishMsg(2, get_geometry_msgs_TwistStamped(json_msg));
    }
    else if (topicName == "/base/twist_mux/command_teleop_override_joy")
    {
        publishMsg(3, get_geometry_msgs_TwistStamped(json_msg));
    }
    else if (topicName == PUB_TOPIC_PERIPH_TELEOP_INFO)
    {
        publishMsg(4, get_agv_msgs_PeriphTeleop(json_msg));
    }
    else
    {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "topicName %s is not support!", topicName.c_str());
    }
}

bool CServiceOperate::init()
{
    RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "Class ServiceOperate init start !");
    if (m_cServiceOperateInitOk)
    {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Class ServiceOperate is already init");
        return false;
    }
    m_srvClients.resize(MAX_CLIENT_NUM);
    m_srvClients[0] = m_nh->create_client<agv_srvs::srv::DynParam>(SERVICE_DYN_PARAM);
    m_srvClients[1] = m_nh->create_client<agv_srvs::srv::StaticParam>(SERVICE_STATIC_PARAM);
    m_srvClients[2] = m_nh->create_client<agv_srvs::srv::ChangeNavType>(SERVICE_CHANGE_NAV_TYPE);
    m_srvClients[3] = m_nh->create_client<agv_srvs::srv::LocalOrder>(SERVICE_LOCAL_ORDER);
    m_srvClients[4] = m_nh->create_client<agv_srvs::srv::InstantActions>(SERVICE_VDA5050_INSTANTACTIONS);
    m_srvClients[5] = m_nh->create_client<agv_srvs::srv::LidarCalib>(SERVICE_LIDAR_CALIBRATION_SERVICE);
    m_srvClients[6] = m_nh->create_client<agv_srvs::srv::QRCalib>(SERVICE_QR_CALIBRATION_SERVICE);
    m_srvClients[7] = m_nh->create_client<agv_srvs::srv::SetLaserCalib>(SERVICE_SET_LASER_CALIB);
    m_srvClients[8] = m_nh->create_client<agv_srvs::srv::OdomCalib>(SERVICE_AGV_CALIB_ODOM_CALIB);
    m_srvClients[9] = m_nh->create_client<agv_srvs::srv::GetMapList>(SERVICE_GET_REFLECTOR_MAP_LISTS);
    // m_srvClients[10] = m_nh->create_client<reflector_slam_ros::srv::reflector_NavTask>(SERVICE_START_REFLECTOR_NAV);
    // m_srvClients[11] = m_nh->create_client<reflector_slam_ros::srv::save_reflector_map>(SERVICE_SAVE_REFLECTOR_MAP);
    m_srvClients[12] = m_nh->create_client<agv_srvs::srv::Relocate>(SERVICE_SET_REFLECTOR_INITIALPOSE);
    m_srvClients[13] = m_nh->create_client<agv_srvs::srv::SetOperationMode>(SERVICE_OPERATION_MODE);
    m_srvClients[14] = m_nh->create_client<agv_srvs::srv::FileTransform>(SERVICE_PARAM_CONFIG);
    m_srvClients[15] = m_nh->create_client<agv_srvs::srv::OutputInterface>(SERVICE_OUTPUTINTERFACE);
    m_srvClients[16] = m_nh->create_client<agv_srvs::srv::LogTransform>(SERVICE_LOG_CONFIG);
    m_srvClients[17] = m_nh->create_client<std_srvs::srv::SetBool>(SERVICE_COMPRESS_DATA);
    m_srvClients[18] = m_nh->create_client<agv_srvs::srv::EditMap>(SERVICE_EDIT_MAP);
    m_srvClients[19] = m_nh->create_client<agv_srvs::srv::SetLoactionMode>(SERVICE_LOCATION_MODE);
    m_srvClients[20] = m_nh->create_client<agv_srvs::srv::SaveMap>(SERVICE_SAVE_MAP);
    m_srvClients[21] = m_nh->create_client<agv_srvs::srv::Relocate>(SERVICE_AGV_RELOCATE);
    m_srvClients[22] = m_nh->create_client<agv_srvs::srv::AutoCalib>(SERVICE_START_AUTOCALIB);
    m_srvClients[23] = m_nh->create_client<agv_srvs::srv::LaserFilter>(SERVICE_SET_FILTER_PARAM);
    m_srvClients[24] = m_nh->create_client<agv_srvs::srv::GetMapPoint>(SERVICE_GET_MAP_PNG);
    m_srvClients[25] = m_nh->create_client<agv_srvs::srv::GetMapList>(SERVICE_GET_MAP_LIST);
    m_srvClients[26] = m_nh->create_client<slam_ros_msgs::srv::ChangeMap>(SERVICE_CHANGE_MAP);
    m_srvClients[27] = m_nh->create_client<slam_ros_msgs::srv::GetLandmarkLists>(SERVICE_GET_LANDMARKLISTS);
    m_srvClients[28] = m_nh->create_client<agv_srvs::srv::Version>(SERVICE_FW_VERSION);
    m_srvClients[29] = m_nh->create_client<agv_srvs::srv::FirmwareUpdate>(SERVICE_FW_UPDATE);
    m_srvClients[30] = m_nh->create_client<agv_srvs::srv::FtpRequest>(SERVICE_FTP_REQUEST);
    m_srvClients[31] = m_nh->create_client<slam_ros_msgs::srv::Syncmap>(SERVICE_SYNC_SLAM_MAP);
    m_srvClients[32] = m_nh->create_client<slam_ros_msgs::srv::Syncmap>(SERVICE_SYNC_REFLECTOR_SLAM_MAP);
    m_srvClients[33] = m_nh->create_client<agv_srvs::srv::AvoidObstaUpdate>(SERVICE_AGV_AVOID_UPDATE);
    m_srvClients[34] = m_nh->create_client<agv_srvs::srv::AvoidObstaSwitch>(SERVICE_AGV_OBSTACLE_AVOIDANCE_SERVER);

    m_srvNameList.reserve(MAX_CLIENT_NUM + 2); // 读写参数服务器
    for (auto &client : m_srvClients)
    {
        if (!client) continue;
        std::string srvName = client->get_service_name();
        RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "---init serviceClient: %s", srvName.c_str());
        m_srvNameList.push_back(srvName);
    }
    m_srvNameList.push_back(SERVICE_GET_PARAM);
    m_srvNameList.push_back(SERVICE_SET_PARAM);

    m_publishers.resize(5);
    // 发布话题
    m_publishers[0] = m_nh->create_publisher<std_msgs::msg::Bool>(PUB_TOPIC_HALFAUTO_MOVE, 1);
    m_publishers[1] = m_nh->create_publisher<geometry_msgs::msg::TwistStamped>(PUB_TOPIC_KEYBOARD_VEL, 1);
    m_publishers[2] = m_nh->create_publisher<geometry_msgs::msg::TwistStamped>(PUB_TOPIC_KEYBOARD_OVERRIDE, 1);
    m_publishers[3] = m_nh->create_publisher<geometry_msgs::msg::TwistStamped>(PUB_TOPIC_KEYBOARD_TELEOP, 1);
    m_publishers[4] = m_nh->create_publisher<agv_msgs::msg::PeriphTeleop>(PUB_TOPIC_PERIPH_TELEOP_INFO, 1);

    m_cServiceOperateInitOk = true;
    RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "Class ServiceOperate init finish !");
    return true;
}

Json::Value CServiceOperate::callService(const std::string &srvName, const Json::Value &json_args, bool &result)
{
    if (std::find(m_srvNameList.begin(), m_srvNameList.end(), srvName) == m_srvNameList.end())
    {
        return Json::Value("agv dose not support bridging for service:" + srvName);
    }
    else if (srvName == SERVICE_DYN_PARAM)
    {
        return callSrv_DynParam(0, json_args, result);
    }
    else if (srvName == SERVICE_STATIC_PARAM)
    {
        return callSrv_StaticParam(1, json_args, result);
    }
    else if (srvName == SERVICE_CHANGE_NAV_TYPE)
    {
        return callSrv_ChangeNavType(2, json_args, result);
    }
    else if (srvName == SERVICE_LOCAL_ORDER)
    {
        return callSrv_V_localOrder(3, json_args, result);
    }
    else if (srvName == SERVICE_VDA5050_INSTANTACTIONS)
    {
        return callSrv_V_instantActions(4, json_args, result);
    }
    else if (srvName == SERVICE_LIDAR_CALIBRATION_SERVICE)
    {
        return callSrv_LidarCalib(5, json_args, result);
    }
    else if (srvName == SERVICE_QR_CALIBRATION_SERVICE)
    {
        return callSrv_QRCalib(6, json_args, result);
    }
    else if (srvName == SERVICE_SET_LASER_CALIB)
    {
        return callSrv_SetLaserCalib(7, json_args, result);
    }
    else if (srvName == SERVICE_AGV_CALIB_ODOM_CALIB)
    {
        return callSrv_OdomCalib(8, json_args, result);
    }
    else if (srvName == SERVICE_GET_REFLECTOR_MAP_LISTS)
    {
        return callSrv_GetMapList(9, json_args, result);
    }
    // else if (srvName == SERVICE_START_REFLECTOR_NAV)
    // {
    //     return callSrv_reflector_NavTask(10, json_args, result);
    // }
    // else if (srvName == SERVICE_SAVE_REFLECTOR_MAP)
    // {
    //     return callSrv_save_reflector_map(11, json_args, result);
    // }
    else if (srvName == SERVICE_SET_REFLECTOR_INITIALPOSE)
    {
        return callSrv_Relocate(12, json_args, result);
    }
    else if (srvName == SERVICE_OPERATION_MODE)
    {
        return callSrv_SetOperationMode(13, json_args, result);
    }
    else if (srvName == SERVICE_PARAM_CONFIG)
    {
        return callSrv_file_transform(14, json_args, result);
    }
    else if (srvName == SERVICE_OUTPUTINTERFACE)
    {
        return callSrv_OutputInterface(15, json_args, result);
    }
    else if (srvName == SERVICE_LOG_CONFIG)
    {
        return callSrv_log_transform(16, json_args, result);
    }
    else if (srvName == SERVICE_COMPRESS_DATA)
    {
        return callSrv_SetBool(17, json_args, result);
    }
    else if (srvName == SERVICE_EDIT_MAP)
    {
        return callSrv_EditMap(18, json_args, result);
    }
    else if (srvName == SERVICE_LOCATION_MODE)
    {
        return callSrv_SetLoactionMode(19, json_args, result);
    }
    else if (srvName == SERVICE_SAVE_MAP)
    {
        return callSrv_SaveMap(20, json_args, result);
    }
    else if (srvName == SERVICE_AGV_RELOCATE)
    {
        return callSrv_Relocate(21, json_args, result);
    }
    else if (srvName == SERVICE_START_AUTOCALIB)
    {
        return callSrv_AutoCalib(22, json_args, result);
    }
    else if (srvName == SERVICE_SET_FILTER_PARAM)
    {
        return callSrv_LaserFilter(23, json_args, result);
    }
    else if (srvName == SERVICE_GET_MAP_PNG)
    {
        return callSrv_GetMapPoint(24, json_args, result);
    }
    else if (srvName == SERVICE_GET_MAP_LIST)
    {
        return callSrv_GetMapList(25, json_args, result);
    }
    else if (srvName == SERVICE_CHANGE_MAP)
    {
        return callSrv_ChangeMap(26, json_args, result);
    }
    else if (srvName == SERVICE_GET_LANDMARKLISTS)
    {
        return callSrv_GetLandmarkLists(27, json_args, result);
    }
    else if (srvName == SERVICE_FW_VERSION)
    {
        return callSrv_Version(28, json_args, result);
    }
    else if (srvName == SERVICE_FW_UPDATE)
    {
        return callSrv_FirmwareUpdate(29, json_args, result);
    }
    else if (srvName == SERVICE_FTP_REQUEST)
    {
        return callSrv_FtpRequest(30, json_args, result);
    }
    else if (srvName == SERVICE_SYNC_SLAM_MAP)
    {
        return callSrv_Syncmap(31, json_args, result);
    }
    else if (srvName == SERVICE_SYNC_REFLECTOR_SLAM_MAP)
    {
        return callSrv_Syncmap(32, json_args, result);
    }
    else if (srvName == SERVICE_AGV_AVOID_UPDATE)
    {
        return callSrv_AvoidObstaUpdate(33, json_args, result);
    }
    else if (srvName == SERVICE_AGV_OBSTACLE_AVOIDANCE_SERVER)
    {
        return callSrv_AvoidObstaSwitch(34, json_args, result);
    }
    else if (srvName == SERVICE_GET_PARAM)
    {
        return ros_param_get(json_args, result);
    }
    else if (srvName == SERVICE_SET_PARAM)
    {
        return ros_param_set(json_args, result);
    }

    return Json::Value("no response");
}

void CServiceOperate::callFaildProgram(const int srvIndex, Json::Value &json_values)
{
    // 打印可能的失败原因
    std::string str_srv = "Service “" + m_srvNameList.at(srvIndex) + "”";
    if (!m_srvClients[srvIndex]->service_is_ready())
    {
        str_srv += " is not exist!";
    }
    else
    {
        str_srv += " return false !";
    }

    json_values = str_srv.c_str();
}

template <typename T>
Json::Value CServiceOperate::callSrv_gen_success_message(
    typename T::Request::SharedPtr request, 
    const int srvIndex, bool &result)
{
    Json::Value json_values;
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<T>>(m_srvClients[srvIndex]);
        auto response_future = client->async_send_request(request);
        response_future.wait_for(std::chrono::seconds(5)); 
        if (response_future.valid()) {
            auto response = response_future.get();
            json_values["success"] = response->success;
            json_values["message"] = response->message;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
   
    return json_values;
}

template <typename T>
Json::Value CServiceOperate::callSrv_gen_success_error(
    typename T::Request::SharedPtr request, 
    const int srvIndex, bool &result)
{
    Json::Value json_values;
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<T>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["success"] = response->success;
            json_values["error"] = response->error;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

template <typename T>
Json::Value CServiceOperate::callSrv_gen_status(
    typename T::Request::SharedPtr request, 
    const int srvIndex, bool &result)
{
    Json::Value json_values;
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<T>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["status"] = response->status;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

template <typename T>
Json::Value CServiceOperate::callSrv_gen_status_msg(
    typename T::Request::SharedPtr request, 
    const int srvIndex, bool &result)
{
    Json::Value json_values;
    result = false;

    try {
        auto client = std::static_pointer_cast<rclcpp::Client<T>>(m_srvClients[srvIndex]);
        if (!client) {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Service client at index %d is null.", srvIndex);
            callFaildProgram(srvIndex, json_values);
            return json_values;
        }
        
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));

        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["status"] = agv_msgs_to_json(response->status);
            result = true;
            RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "Service call successful.");
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Service call timed out or failed.");
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Exception while calling service: %s", e.what());
        callFaildProgram(srvIndex, json_values);
    }

    return json_values;
}

Json::Value CServiceOperate::callSrv_DynParam(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"node_name", "param_key", "param_value"};
    for (auto &field : fields) {
        if (!json_args.isMember(field)) {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    // 调用服务
    Json::Value json_values;
    auto request = get_agv_srvs_DynParam(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::DynParam>>(m_srvClients[srvIndex]);
        auto response_future = client->async_send_request(request);
        response_future.wait_for(std::chrono::seconds(5)); 
        if (response_future.valid()) {
            auto response = response_future.get();
            json_values["ack_message"] = response->ack_message;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }

    return json_values;
}
Json::Value CServiceOperate::callSrv_StaticParam(const int srvIndex, const Json::Value &json_args, bool &result)
{
    Json::Value json_values;
    result = false; 

    if (!json_args.isMember("action_name")) {
        json_values = "field \"action_name\" is not input!";
        return json_values;
    }
    
    auto request = std::make_shared<agv_srvs::srv::StaticParam::Request>();
    request->action_name = json_args["action_name"].asString();
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::StaticParam>>(
            m_srvClients[srvIndex]
        );
        
        if (!client) {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Invalid client for index: %d", srvIndex);
            json_values["error"] = "Invalid client";
            return json_values;
        }
        
        auto future = client->async_send_request(request);
        auto status = future.wait_for(std::chrono::seconds(5));
        
        if (status == std::future_status::ready) {
            auto response = future.get();
            
            const char* ID = std::getenv("Robot_ID");
            std::string robot_id_str = ID ? ID : ""; // 确保不为空
            
            json_values["ack_message"] = response->ack_message;
            json_values["Robot_ID"] = robot_id_str;
            
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), 
                        "Service call timed out for index: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), 
                    "Service call failed for index %d: %s", srvIndex, e.what());
        callFaildProgram(srvIndex, json_values);
    }

    return json_values;
}

Json::Value CServiceOperate::callSrv_ChangeNavType(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("type"))
    {
        return Json::Value("field “type” is not input!");
    }

    // 调用服务
    auto request = get_agv_srvs_ChangeNavType(json_args);
    return callSrv_gen_success_message<agv_srvs::srv::ChangeNavType>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_V_localOrder(const int srvIndex, const Json::Value &json_args, bool &result)
{
    // 调用服务
    auto request = get_agv_srvs_V_localOrder(json_args);
    return callSrv_gen_success_message<agv_srvs::srv::LocalOrder>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_V_instantActions(const int srvIndex, const Json::Value &json_args, bool &result)
{
    // 调用服务
    auto request = get_agv_srvs_V_instantActions(json_args);
    return callSrv_gen_success_message<agv_srvs::srv::InstantActions>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_LidarCalib(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[7] = {"trigger", "node0", "node1", "node2", "node3", "node4", "node5"};
    for (auto &field : fields) {
        if (!json_args.isMember(field)) {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    Json::Value json_values;
    auto request = get_agv_srvs_LidarCalib(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::LidarCalib>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["success"] = response->success;
            json_values["msg"] = response->msg;
            json_values["x"] = response->x;
            json_values["y"] = response->y;
            json_values["angle"] = response->angle;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_QRCalib(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[11] = {"trigger", "node0", "node1", "node2", "node3", "node4", "node5", "node6", "node7", "node8", "node9"};
    for (auto &field : fields) {
        if (!json_args.isMember(field)) {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    Json::Value json_values;
    auto request = get_agv_srvs_QRCalib(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::QRCalib>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["success"] = response->success;
            json_values["msg"] = response->msg;
            json_values["angle"] = response->angle;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_SetLaserCalib(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[6] = {"laser_num", "x", "y", "yaw", "enable", "isInversion"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    // 调用服务
    auto request = get_agv_srvs_SetLaserCalib(json_args);
    return callSrv_gen_success_error<agv_srvs::srv::SetLaserCalib>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_OdomCalib(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[4] = {"start", "model", "vel", "oprange"};
    for (auto &field : fields) {
        if (!json_args.isMember(field)) {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    Json::Value json_values;
    auto request = get_agv_srvs_OdomCalib(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::OdomCalib>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["status"] = response->status;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_GetMapList(const int srvIndex, const Json::Value &json_args, bool &result)
{
   if (!json_args.isMember("value")) {
        return Json::Value("field “value” is not input!");
    }

    Json::Value json_values;
    auto request = get_agv_srvs_GetMapList(json_args);
    result = false; 
    
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::GetMapList>>(m_srvClients[srvIndex]);
        if (!client) {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Service client at index %d is null.", srvIndex);
            callFaildProgram(srvIndex, json_values);
            return json_values;
        }

        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5)); 

        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["status"] = agv_msgs_to_json(response->status);
            Json::Value json_mapIdsArray(Json::arrayValue);
            for (auto &it : response->mapids) {
                json_mapIdsArray.append(it);
            }
            json_values["mapIDs"] = json_mapIdsArray;
            result = true;
            RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "Service call successful.");
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Service call timed out or failed.");
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Exception while calling service: %s", e.what());
        callFaildProgram(srvIndex, json_values);
    }

    return json_values;
}

// Json::Value CServiceOperate::callSrv_reflector_NavTask(const int srvIndex, const Json::Value &json_args, bool &result)
// {
//     if (!json_args.isMember("mode"))
//     {
//         return Json::Value("field “mode” is not input!");
//     }
//     if (!json_args.isMember("mapID"))
//     {
//         return Json::Value("field “mapID” is not input!");
//     }

//     // 调用服务
//     Json::Value json_values;
//     reflector_slam_ros::srv::reflector_NavTask srv = get_reflector_slam_ros_reflector_NavTask(json_args);
//     if (m_srvClients[srvIndex].call(srv))
//     {
//         json_values["code"] = (bool)srv.response.code;
//         json_values["info"] = srv.response.info;
//         json_values["status"] = (bool)srv.response.status;
//         result = true;
//     }
//     else
//     {
//         callFaildProgram(srvIndex, json_values);
//     }
//     return json_values;
// }

// Json::Value CServiceOperate::callSrv_save_reflector_map(const int srvIndex, const Json::Value &json_args, bool &result)
// {
//     std::string fields[3] = {"mapID", "mode", "reflector"};
//     for (auto &field : fields)
//     {
//         if (!json_args.isMember(field))
//         {
//             return Json::Value("field “" + field + "” is not input!");
//         }
//     }

//     // 调用服务
//     Json::Value json_values;
//     reflector_slam_ros::srv::save_reflector_map srv = get_reflector_slam_ros_save_reflector_map(json_args);
//     if (m_srvClients[srvIndex].call(srv))
//     {
//         json_values["flag"] = (bool)srv.response.flag;
//         json_values["info"] = srv.response.info;
//         result = true;
//     }
//     else
//     {
//         callFaildProgram(srvIndex, json_values);
//     }
//     return json_values;
// }

Json::Value CServiceOperate::callSrv_Relocate(const int srvIndex, const Json::Value &json_args, bool &result)
{
    // 调用服务
    auto request = get_agv_srvs_Relocate(json_args);
    return callSrv_gen_success_error<agv_srvs::srv::Relocate>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_SetOperationMode(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"mode", "submode", "sender"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    // 调用服务
    auto request = get_agv_srvs_SetOperationMode(json_args);
    return callSrv_gen_success_message<agv_srvs::srv::SetOperationMode>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_file_transform(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("file_name"))
    {
        return Json::Value("field “file_name” is not input!");
    }
    if (!json_args.isMember("recfilesize"))
    {
        return Json::Value("field “recfilesize” is not input!");
    }

    Json::Value json_values;
    auto request = get_agv_ftp_log_file_transform(json_args);
    
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::FileTransform>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["status"] = response->status;
            json_values["sendfilesize"] = response->sendfilesize;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Call service timed out for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Exception in callSrv_file_transform: %s", e.what());
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_OutputInterface(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("index_level")) {
        return Json::Value("field “index_level” is not input!");
    }

    Json::Value json_values;
    auto request = get_agv_srvs_OutputInterface(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::OutputInterface>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["status"] = response->state;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_log_transform(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("log_status"))
    {
        return Json::Value("field \"log_status\" is not input!");
    }

    Json::Value json_values;
    auto request = get_agv_srvs_log_transform(json_args);
    
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::LogTransform>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["log_filename"] = response->log_filename;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Call service timed out for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Exception in callSrv_log_transform: %s", e.what());
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_SetBool(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("data"))
    {
        return Json::Value("field “data” is not input!");
    }

    // 调用服务
    Json::Value json_values;
    auto request = get_std_srvs_SetBool(json_args);
    return callSrv_gen_success_message<std_srvs::srv::SetBool>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_EditMap(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"mapID", "earse_action", "transform_action"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    // 调用服务
    auto request = get_agv_srvs_EditMap(json_args);
    return callSrv_gen_status_msg<agv_srvs::srv::EditMap>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_SetLoactionMode(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"mode", "mapId", "resolution"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    // 调用服务
    auto request = get_agv_srvs_SetLoactionMode(json_args);
    return callSrv_gen_status_msg<agv_srvs::srv::SetLoactionMode>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_SaveMap(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("name"))
    {
        return Json::Value("field “name” is not input!");
    }
    
    // 调用服务
    auto request = get_agv_srvs_SaveMap(json_args);
    return callSrv_gen_status_msg<agv_srvs::srv::SaveMap>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_AutoCalib(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"calib_cmd", "seconds", "code"};
    for (auto &field : fields) {
        if (!json_args.isMember(field)) {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    Json::Value json_values;
    auto request = get_agv_srvs_AutoCalib(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::AutoCalib>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["code"] = response->code;
            json_values["status"] = response->status;
            json_values["message"] = response->message;
            json_values["x"] = response->x;
            json_values["y"] = response->y;
            json_values["yaw"] = response->yaw;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_LaserFilter(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"type", "invert", "points"};
    for (auto &field : fields) {
        if (!json_args.isMember(field)) {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    Json::Value json_values;
    auto request = get_agv_srvs_LaserFilter(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::LaserFilter>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["success"] = response->success;
            json_values["msg"] = response->msg;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_GetMapPoint(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("status")) {
        return Json::Value("field “status” is not input!");
    }

    Json::Value json_values;
    auto request = get_agv_srvs_GetMapPoint(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::GetMapPoint>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["resolution"] = response->resolution;
            json_values["width"] = response->width;
            json_values["height"] = response->height;
            json_values["origin"] = agv_msgs_to_json(response->origin);
            json_values["data"] = response->data;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}


Json::Value CServiceOperate::callSrv_ChangeMap(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[4] = {"code", "mapId", "mapNewId", "Keys"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }

    // 调用服务
    auto request = get_slam_ros_msgs_ChangeMap(json_args);
    return callSrv_gen_success_error<slam_ros_msgs::srv::ChangeMap>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_GetLandmarkLists(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("cmd")) {
        return Json::Value("field “cmd” is not input!");
    }

    Json::Value json_values;
    auto request = get_slam_ros_msgs_GetLandmarkLists(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<slam_ros_msgs::srv::GetLandmarkLists>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            Json::Value json_landmark_poseArray(Json::arrayValue);
            for (auto &it : response->landmark_pose) {
                json_landmark_poseArray.append(agv_msgs_to_json(it));
            }
            Json::Value json_colorsArray(Json::arrayValue);
            for (auto &it : response->colors) {
                json_colorsArray.append(agv_msgs_to_json(it));
            }

            json_values["landmark_pose"] = json_landmark_poseArray;
            json_values["scale"] = agv_msgs_to_json(response->scale);
            json_values["colors"] = json_colorsArray;
            json_values["status"] = (bool)response->status;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_Version(const int srvIndex, const Json::Value &json_args, bool &result)
{
    Json::Value json_values;
    auto request = get_agv_srvs_Version(json_args);
    try {
        auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::Version>>(m_srvClients[srvIndex]);
        auto future = client->async_send_request(request);
        auto wait_result = future.wait_for(std::chrono::seconds(5));
        
        if (wait_result == std::future_status::ready) {
            auto response = future.get();
            json_values["ver"] = response->ver;
            json_values["branch"] = response->branch;
            json_values["hash"] = response->hash;
            result = true;
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed for srvIndex: %d", srvIndex);
            callFaildProgram(srvIndex, json_values);
        }
    } catch (const std::exception &e) {
        callFaildProgram(srvIndex, json_values);
    }
    return json_values;
}

Json::Value CServiceOperate::callSrv_FirmwareUpdate(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[5] = {"fw_size", "fw_name", "fw_md5", "fw_content", "is_test_version"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }
    // 调用服务
    auto request = get_agv_srvs_FirmwareUpdate(json_args);
    return callSrv_gen_success_message<agv_srvs::srv::FirmwareUpdate>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_FtpRequest(const int srvIndex, const Json::Value &json_args, bool &result)
{
    std::string fields[3] = {"type", "user", "passwd"};
    for (auto &field : fields)
    {
        if (!json_args.isMember(field))
        {
            return Json::Value("field “" + field + "” is not input!");
        }
    }
    // 调用服务
    auto request = get_agv_srvs_FtpRequest(json_args);
    return callSrv_gen_success_message<agv_srvs::srv::FtpRequest>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_Syncmap(const int srvIndex, const Json::Value &json_args, bool &result)
{
    // std::string fields[3] = {"opCode", "mapId", "md5Code"};
    // for (auto &field : fields)
    // {
    //     if (!json_args.isMember(field))
    //     {
    //         return Json::Value("field “" + field + "” is not input!");
    //     }
    // }
    // 调用服务
    Json::Value json_values;
    // auto request = get_slam_ros_msgs_Syncmap(json_args);

    // try {
    //     using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::Syncmap>::SharedFuture;
    //     auto response_received_callback = [this, &json_values, &result, srvIndex](ServiceResponseFuture future) {
    //         if (future.valid()) {
    //             auto response = future.get(); 
    //             json_values["success"] = (bool)response->success;
    //             json_values["error"] = response->error;
    //             json_values["md5Code"] = response->md5Code;
    //             result = true; 
    //             RCLCPP_INFO(rclcpp::get_logger("AgvTcpBridge"), "call service success.");
    //         } else {
    //             RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "call service failed.");
    //             callFaildProgram(srvIndex, json_values);
    //         }
    //     };
    //     auto client = std::static_pointer_cast<rclcpp::Client<agv_srvs::srv::Syncmap>>(m_srvClients[srvIndex]);
    //     auto result = client->async_send_request(request, response_received_callback);
    // } catch (const std::exception &e) {
    //     callFaildProgram(srvIndex, json_values);
    // }
    
    return json_values;
}

Json::Value CServiceOperate::callSrv_AvoidObstaUpdate(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("start"))
    {
        return Json::Value("field “start” is not input!");
    }
    // 调用服务
    auto request = get_agv_srvs_AvoidObstaUpdate(json_args);
    return callSrv_gen_status<agv_srvs::srv::AvoidObstaUpdate>(request, srvIndex, result);
}

Json::Value CServiceOperate::callSrv_AvoidObstaSwitch(const int srvIndex, const Json::Value &json_args, bool &result)
{
    if (!json_args.isMember("planname"))
    {
        return Json::Value("field “planname” is not input!");
    }
    // 调用服务
    auto request = get_agv_srvs_AvoidObstaSwitch(json_args);
    return callSrv_gen_status<agv_srvs::srv::AvoidObstaSwitch>(request, srvIndex, result);
}

Json::Value CServiceOperate::ros_param_get(const Json::Value &json_args, bool &result)
{
    std::string param_name = json_args["name"].asString();
    if (param_name.empty()) {
        return Json::Value("param name is empty!");
    }

    // 格式化为  开头“/”，末尾无斜杆
    if (param_name.front() != '/') {
        param_name.insert(0, "/");
    }
    if (param_name.back() == '/') {
        param_name.pop_back();
    }
    
    Json::Value json_result;
    json_result["value"] = "param not found!";

    // 读取参数
    if (!SharedParamsInterface::initialize()) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Failed to initialize shared memory interface");
        return json_result;
    }

    try { 
        size_t pos = param_name.find("ROBOT_ID");  
        if (pos != std::string::npos) {
            const char* ID = std::getenv("ROBOT_ID");
            json_result["value"] = ID;
            result = true;
        } else {
            auto params = SharedParamsInterface::getNodeParams("shared_parameters");
            if (params.empty() || 
                (params.find("error_code") != params.end() && 
                    params["error_code"].getValue<int>() > 0)) {
                RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), "Error get shared parameters");
                return json_result;
            } 
            
            auto all_params = SharedParamsInterface::listAllParams();
            for (const auto& param : all_params) {
                pos = param.param_name.find(param_name);  
                if (pos != std::string::npos) {
                    json_result["value"] = param.param_value;
                    result = true;
                }
            }
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("AgvTcpBridge"), "Error reading parameters: " << e.what());
        return json_result;
    }

    return json_result;
}

Json::Value CServiceOperate::ros_param_set(const Json::Value &json_args, bool &result)
{
    // 直接设置
    // std::string param_name = json_args["name"].asString();
    // if (param_name.empty())
    // {
    //     return Json::Value("param name is empty!");
    // }
    // if (!json_args["value"].isString())
    // {
    //     return Json::Value("please input param value as string type!");
    // }

    // std::string str_value = json_args["value"].asCString();
    // // 符合string类型,直接设置并返回
    // if (isStringValue(str_value))
    // {
    //     ros::param::set(param_name, str_value);
    //     result = true;
    //     return Json::Value("param set success! type: string");
    // }

    // // 非string类型，判断是否符合其他类型
    // trimStringValue(str_value); // 其他类型需要去除两端空格
    // std::string str_type = "";
    // if (str_value.empty())
    // {
    //     return Json::Value("param value is empty");
    // }
    // else if (isIntValue(str_value))
    // {
    //     ros::param::set(param_name, std::stoi(str_value));
    //     str_type = "int";
    // }
    // else if (isDoubleValue(str_value))
    // {
    //     ros::param::set(param_name, std::stod(str_value));
    //     str_type = "double";
    // }
    // else if (isBoolValue(str_value))
    // {
    //     ros::param::set(param_name, str_value == "true");
    //     str_type = "bool";
    // }
    // else
    // {
    //     return Json::Value("param value is not string/int/double/bool type!");
    // }

    // result = true;
    return Json::Value("param set success! type: " /*+ str_type*/);
}

// 获取参数值并转换为 Json::Value
Json::Value CServiceOperate::getRosParamValue(const std::string &param_name, bool &success, const bool output_string)
{
    Json::Value json_value;
    // double num_val;
    // std::string string_val;
    // bool bool_val;

    // success = true;
    // if (ros::param::get(param_name, num_val))
    // {
    //     if (output_string)
    //     {
    //         std::stringstream ss;
    //         ss << num_val;
    //         json_value = ss.str();
    //     }
    //     else
    //     {
    //         json_value = num_val;
    //     }

    //     RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"),"Get Numerical param: " << param_name << " = " << num_val);
    // }
    // else if (ros::param::get(param_name, string_val))
    // {
    //     json_value = string_val;
    //     RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"),"Get string param: " << param_name << " = " << string_val);
    // }
    // else if (ros::param::get(param_name, bool_val))
    // {
    //     if (output_string)
    //     {
    //         json_value = bool_val ? "true" : "false";
    //     }
    //     else
    //     {
    //         json_value = bool_val;
    //     }
    //     RCLCPP_INFO_STREAM(rclcpp::get_logger("AgvTcpBridge"),"Get bool param: " << param_name << " = " << bool_val);
    // }
    // else
    // {
    //     json_value = "param type is not support!";
    //     success = false;
    // }

    return json_value;
}

// 递归构建 Json::Value 对象
Json::Value CServiceOperate::buildRosParamJsonForNode(int &obtained_num, const std::string &node_namespace, const std::vector<std::string> &name_list)
{
    Json::Value json_node;
    // for (const auto &name : name_list)
    // {
    //     if (name.find(node_namespace) == 0)
    //     {
    //         std::string relative_name = name.substr(node_namespace.length());
    //         if (!relative_name.empty() && relative_name[0] == '/')
    //         {
    //             relative_name = relative_name.substr(1);
    //             size_t next_slash = relative_name.find('/');
    //             if (next_slash == std::string::npos)
    //             {
    //                 // 是参数
    //                 bool get_success = false;
    //                 Json::Value json_value = getRosParamValue(name, get_success,false);
    //                 if (get_success)
    //                 {
    //                     json_node[relative_name] = json_value;
    //                     obtained_num++;
    //                 }
    //             }
    //             else
    //             {
    //                 // 是子节点
    //                 std::string sub_node_name = relative_name.substr(0, next_slash);
    //                 std::string sub_node_namespace = node_namespace + "/" + sub_node_name;
    //                 if (json_node[sub_node_name].isNull())
    //                 {
    //                     json_node[sub_node_name] = buildRosParamJsonForNode(obtained_num, sub_node_namespace, name_list);
    //                 }
    //             }
    //         }
    //     }
    // }
    return json_node;
}

// 删除字符串两端的空格
void CServiceOperate::trimStringValue(std::string &str_value)
{
    while (!str_value.empty() && str_value.front() == '"')
    {
        str_value.erase(0, 1);
    }
    while (!str_value.empty() && str_value.back() == '"')
    {
        str_value.pop_back();
    }
}

// 判断参数设置值类型
bool CServiceOperate::isStringValue(std::string &str_value)
{
    bool result = false;
    if (str_value.front() == '\"' && str_value.back() == '\"')
    {
        // 两端引号仅做判断使用，不做实际设置
        str_value = str_value.substr(1, str_value.length() - 2);
        result = true;
    }
    return result;
}

bool CServiceOperate::isIntValue(const std::string &str_value)
{
    return std::all_of(str_value.begin(), str_value.end(), ::isdigit);
}

bool CServiceOperate::isDoubleValue(const std::string &str_value)
{
    std::regex doubleRegex(R"(^-?\d+\.\d+$)");
    return std::regex_match(str_value, doubleRegex);
}

bool CServiceOperate::isBoolValue(const std::string &str_value)
{
    return str_value == "true" || str_value == "false";
}

/*-----------------------话题发布-----------------------*/
template <typename T>
void CServiceOperate::publishMsg(const int index, const T &msg)
{
    // m_publishers[index]->publish(msg);
    try {
  
        if (index < 0 || index >= static_cast<int>(m_publishers.size())) {
            throw std::out_of_range("Publisher index out of range");
        }

        auto publisher = std::dynamic_pointer_cast<rclcpp::Publisher<T>>(
            m_publishers[index]
        );

        if (!publisher) {
            throw std::runtime_error("Type mismatch for publisher at index " + 
                                   std::to_string(index));
        }

        if (!publisher->get_subscription_count()) {
            RCLCPP_WARN(rclcpp::get_logger("AgvTcpBridge"), 
                       "No subscribers for publisher at index %d", index);
        }

        publisher->publish(msg);

    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("AgvTcpBridge"), 
                    "Failed to publish: %s", e.what());
    }  
}